python n维向量向任意方向旋转,求旋转矩阵

本来想做绕轴旋转,绕轴旋转是在垂直于轴向量的空间里旋转,但是n维空间里与某个向量垂直的空间为n-1维,而旋转只在二维空间里有定义。所以这里就改成了,向任意方向旋转。

计算单位向量 X = ( x 1 , x 2 , ⋯   , x n ) X=(x_1,x_2,\cdots,x_n) X=(x1,x2,,xn)旋转到单位向量 V = ( v 1 , v 2 , ⋯   , v n ) V=(v_1,v_2,\cdots,v_n) V=(v1,v2,,vn)的旋转矩阵 R R R

旋转坐标系

任意的旋转都可以看作绕着一个轴,在某个平面上的旋转。
不失一般性,假定向量 V = ( v 1 , v 2 , ⋯   , v n ) V=(v_1,v_2,\cdots,v_n) V=(v1,v2,,vn) v 1 × v 2 v_1\times v_2 v1×v2张成的平面上旋转,用矩阵乘法可表示为: V ′ = R 1 V V'=R_1V V=R1V其中旋转矩阵 R 1 R_1 R1定义为
R 1 = ( cos ⁡ α − sin ⁡ α sin ⁡ α cos ⁡ α 1 ⋱ 1 ) R_1=\left( \begin{array}{ccc} \cos\alpha& -\sin\alpha& & & \\ \sin\alpha& \cos\alpha& & & \\ & &1 & & \\ & & & \ddots & \\ & & & & 1 \\ \end{array} \right) R1=cosαsinαsinαcosα11
其中 cos ⁡ α = v 2 v 1 2 + v 2 2 \cos\alpha=\frac{v_2}{\sqrt{v_1^2+v_2^2}} cosα=v12+v22 v2 sin ⁡ α = v 1 v 1 2 + v 2 2 \sin\alpha=\frac{v_1}{\sqrt{v_1^2+v_2^2}} sinα=v12+v22 v1
由此我们将向量 V V V旋转到与 ( v 1 , 0 , ⋯   , 0 ) (v_1,0,\cdots,0) (v1,0,,0)正交。得 V ′ = ( 0 , v 1 2 + v 2 2 , v 3 , v 4 , ⋯   , v n ) V'=(0,\sqrt{v_1^2+v_2^2}, v_3,v_4,\cdots,v_n) V=(0,v12+v22 ,v3,v4,,vn)
以此类推,将 V V V旋转到 ( 0 , ⋯   , 0 , 1 ) (0,\cdots,0,1) (0,,0,1)需要 n − 1 n-1 n1次旋转。将这些旋转变换组合为一个旋转矩阵 R 0 = R n − 1 R n − 2 ⋯ R 1 R_0=R_{n-1}R_{n-2}\cdots R_1 R0=Rn1Rn2R1(注意,乘法不满足交换律,顺序不能颠倒)。顺便我们可以计算出这个旋转的逆矩阵 R 0 − 1 = R 1 ⊤ ⋯ R n − 2 ⊤ R n − 1 ⊤ R_0^{-1}=R_1^\top\cdots R_{n-2}^\top R_{n-1}^\top R01=R1Rn2Rn1

旋转向量

现在我们要做的是将 R 0 X R_0X R0X旋转到坐标轴 ( 0 , ⋯   , 0 , 1 ) (0,\cdots,0,1) (0,,0,1),并求旋转矩阵。做法与上面相同。最后可以求得,在旋转后的坐标系下,旋转矩阵为 R X R_X RX: R 0 V = R X R 0 X R_0V=R_XR_0X R0V=RXR0X

虽然 R R R R X R_X RX表示的旋转角度相同,但是这两个旋转所在的坐标系是不同的,还需要将 R X R_X RX的坐标轴旋转回去:
R = R 0 − 1 R X R 0 R=R_0^{-1}R_XR_0 R=R01RXR0

代码实现

import numpy as np

def Rot_map(V):
	assert len(V.shape) == 1
	assert np.linalg.norm(V) - 1 < 1e-8
	n_dim = V.shape[0]
    Rot = np.eye(n_dim)
    Rot_inv = np.eye(n_dim)
    for rotate in range(n_dim-1):
        rot_mat = np.eye(n_dim)
        rot_norm = np.sqrt(V[rotate]**2 + V[rotate+1]**2)
        cos_theta = V[rotate+1]/rot_norm
        sin_theta = V[rotate]/rot_norm
        rot_mat[rotate,rotate] = cos_theta
        rot_mat[rotate,rotate+1] = - sin_theta
        rot_mat[rotate+1,rotate] = sin_theta
        rot_mat[rotate+1,rotate+1] = cos_theta

        V = np.dot(rot_mat, V)

        Rot = np.dot(rot_mat, Rot)
        Rot_inv = np.dot(Rot_inv,rot_mat.transpose())
    return Rot, Rot_inv
    
n_dim = 512

X = np.random.rand(n_dim)
X = X / np.linalg.norm(X)
V = np.random.rand(n_dim)
V = V / np.linalg.norm(V)

R_0, R_0_inv = Rot_map(V)
R_X, _ = Rot_map(np.dot(R_0, X))

R = np.dot(np.dot(R_0_inv, R_X), R_0)

assert np.linalg.norm(np.dot(R,X) - V) < 1e-8

扩展

这个方法可以用来在正交于某个向量的空间中随机采样。例如,在 n n n维空间中,采样与 V = ( v 1 , v 2 , ⋯   , v n ) V=(v_1,v_2,\cdots,v_n) V=(v1,v2,,vn)正交的向量,则代码如下:

import numpy as np

def Rot_map(V):
	assert len(V.shape) == 1
	assert np.linalg.norm(V) - 1 < 1e-8
    z_dim = V.shape[0]
    Rot = np.eye(z_dim)
    Rot_inv = np.eye(z_dim)
    for rotate in range(z_dim-1):
        rot_mat = np.eye(z_dim)
        rot_norm = np.sqrt(V[rotate]**2 + V[rotate+1]**2)
        cos_theta = V[rotate+1]/rot_norm
        sin_theta = V[rotate]/rot_norm
        rot_mat[rotate,rotate] = cos_theta
        rot_mat[rotate,rotate+1] = - sin_theta
        rot_mat[rotate+1,rotate] = sin_theta
        rot_mat[rotate+1,rotate+1] = cos_theta

        V = np.dot(rot_mat, V)

        Rot = np.dot(rot_mat, Rot)
        Rot_inv = np.dot(Rot_inv,rot_mat.transpose())
    return Rot, Rot_inv
    
z_dim = 512
n_samples = 10000

V = np.random.rand(z_dim)
V = V / np.linalg.norm(V)

R_0, R_0_inv = Rot_map(V)

samples = np.random.rand(z_dim-1, n_samples)
samples = np.concatenate((samples, np.zeros((1, n_samples))))
samples = np.dot(R_0_inv, samples)

assert np.mean(np.abs(np.dot(V, samples))) < 1e-8

进一步,我们采样与一组向量 V s Vs Vs正交的向量:

import numpy as np


def gs(X, row_vecs=True, norm = True):
	'''
	https://gist.github.com/iizukak/1287876/edad3c337844fac34f7e56ec09f9cb27d4907cc7
	'''
    if not row_vecs:
        X = X.T
    Y = X[0:1,:].copy()
    for i in range(1, X.shape[0]):
        proj = np.diag((X[i,:].dot(Y.T)/np.linalg.norm(Y,axis=1)**2).flat).dot(Y)
        Y = np.vstack((Y, X[i,:] - proj.sum(0)))
    if norm:
        Y = np.diag(1/np.linalg.norm(Y,axis=1)).dot(Y)
    if row_vecs:
        return Y
    else:
        return Y.T


def Rot_map(V):
	assert len(V.shape) == 1
	assert np.linalg.norm(V) - 1 < 1e-8
    z_dim = V.shape[0]
    Rot = np.eye(z_dim)
    Rot_inv = np.eye(z_dim)
    for rotate in range(z_dim-1):
        rot_mat = np.eye(z_dim)
        rot_norm = np.sqrt(V[rotate]**2 + V[rotate+1]**2)
        cos_theta = V[rotate+1]/rot_norm
        sin_theta = V[rotate]/rot_norm
        rot_mat[rotate,rotate] = cos_theta
        rot_mat[rotate,rotate+1] = - sin_theta
        rot_mat[rotate+1,rotate] = sin_theta
        rot_mat[rotate+1,rotate+1] = cos_theta

        V = np.dot(rot_mat, V)

        Rot = np.dot(rot_mat, Rot)
        Rot_inv = np.dot(Rot_inv,rot_mat.transpose())
    return Rot, Rot_inv
    
z_dim = 10 # number of dimensions
n_vectors= 2 # number of Vs
assert n_vectors < z_dim
n_samples = 10 # number of samples to orthogonal Vs

Vs = np.random.rand(n_vectors, z_dim)
# Gram-Schmidt Orthogonization
orth_Vs = gs(Vs)
orth_Vs = orth_Vs.T

Rots = np.eye(z_dim)
Rot_invs = np.eye(z_dim)
for idx in range(n_vectors):
    V = orth_Vs[:, idx]
    R_0 = np.eye(z_dim)
    R_0_inv = np.eye(z_dim)
    R_0[0:z_dim-idx,0:z_dim-idx], R_0_inv[0:z_dim-idx,0:z_dim-idx] = Rot_map(V[range(z_dim-idx)])
    
    orth_Vs = R_0.dot(orth_Vs)
    Rots = np.dot(R_0, Rots)
    Rot_invs = np.dot(Rot_invs, R_0_inv)

samples = np.random.rand(z_dim-n_vectors, n_samples)
samples = np.concatenate((samples, np.zeros((n_vectors, n_samples))))
samples = np.dot(Rot_invs, samples)

assert np.mean(np.abs(np.dot(Vs, samples))) < 1e-8

用矩阵旋转太慢了,上述采样操作可以用矩阵分解解决(在任意向量的正交空间中采样):

import numpy as np

z_dim = 10 # number of dimensions
n_vectors= 2 # number of Vs
assert n_vectors < z_dim
n_samples = 20 # number of samples to orthogonal Vs

Vs = np.random.rand(n_vectors, z_dim)
Vs = np.concatenate((Vs,np.zeros((z_dim-n_vectors, z_dim))))

U, s, V = np.linalg.svd(Vs)

directions = np.eye(z_dim)
directions[range(n_vectors),range(n_vectors)] = 0
directions = np.dot(np.dot(U, directions), V)

samples = np.random.rand(z_dim, n_samples)
samples = np.dot(directions.T, samples)

assert np.mean(np.abs(np.dot(Vs, samples))) < 1e-8
  • 7
    点赞
  • 12
    收藏
    觉得还不错? 一键收藏
  • 3
    评论
实现Bundle Adjustment需要使用非线性优化算法,常见的有Levenberg-Marquardt算法和Gauss-Newton算法。这里以Gauss-Newton算法为例。 Bundle Adjustment的主要目的是优化相机的内参(如焦距、主点位置等)、相机的外参(如旋转矩阵、平移向量等)、以及三维点的位置。下面给出一个简单的Python实现: ```python import numpy as np from scipy.optimize import least_squares # 假设相机数为N,每个相机有6个参数(3个旋转角度,3个平移向量) # 三维点数为M,每个点有3个坐标值 # measurements为M*N个二维像素坐标值,points_3d为M*3个三维点坐标值 # 定义相机内参和初始相机外参和三维点坐标 K = np.array([[f, 0, cx], [0, f, cy], [0, 0, 1]]) # 内参矩阵 R = np.zeros((N, 3, 3)) # 初始旋转矩阵 t = np.zeros((N, 3)) # 初始平移向量 points_3d = np.random.rand(M, 3) # 初始三维点坐标 # 定义重投影误差函数 def reprojection_error(params, M, N, measurements, points_3d): error = [] K = np.array([[params[0], 0, params[1]], [0, params[0], params[2]], [0, 0, 1]]) for i in range(N): R = cv2.Rodrigues(params[3*i+3:3*i+6])[0] t = params[3*N+3*i:3*N+3*i+3] proj_points = cv2.projectPoints(points_3d, R, t, K, distortion_coeffs=None)[0] error += (proj_points - measurements[:, i, :]).ravel().tolist() return error # 使用Gauss-Newton算法优化相机内参、外参和三维点坐标 params0 = np.hstack((f, cx, cy, R.ravel(), t.ravel(), points_3d.ravel())) res = least_squares(reprojection_error, params0, method='lm', args=(M, N, measurements, points_3d)) # 优化后的旋转矩阵 R_opt = np.zeros((N, 3, 3)) for i in range(N): R_opt[i] = cv2.Rodrigues(res.x[3*i+3:3*i+6])[0] ``` 需要注意的是,上面的实现中使用了OpenCV的`cv2.projectPoints`函数进行重投影计算,需要保证OpenCV已经安装。

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 3
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值